/*******************************************************************************
* FILE NAME: user_routines_fast.c <VEX VERSION>
*
* DESCRIPTION:
*  This file is where the user can add their custom code within the framework
*  of the routines below. 
*
* USAGE:
*  You can either modify this file to fit your needs, or remove it from your 
*  project and replace it with a modified copy. 
*
* OPTIONS:  Interrupts are disabled and not used by default.
*
*******************************************************************************/

#include "ifi_aliases.h"
#include "ifi_default.h"
#include "ifi_utilities.h"
#include "user_routines.h"
#include "delays.h"
#include <stdio.h>
#include "serial_ports.h"
#include "VEXRobot.h"

/*** DEFINE USER VARIABLES AND INITIALIZE THEM HERE ***/

static void (*IntHandler1)(unsigned char) = 0;
static void (*IntHandler2)(unsigned char) = 0;
static void (*IntHandler3)(unsigned char) = 0;
static void (*IntHandler4)(unsigned char) = 0;
static void (*IntHandler5)(unsigned char) = 0;
static void (*IntHandler6)(unsigned char) = 0;

// #define TIMER_INTS
extern unsigned char HiResCounter;

#ifdef TIMER_INTS
static void (*TmrIntHandler1)(unsigned char) = 0;
static void (*TmrIntHandler3)(unsigned char) = 0;
#endif

void SetupInterruptHandler(unsigned char intNum, void (*handler)(unsigned char))
{
   switch(intNum)
   {
   case 1:
      IntHandler1 = handler;
      if(IntHandler1)
      {
         INTCON2bits.INTEDG2 = 0;    /* set rc_dig_int1 falling edge triggered */
         INTCON3bits.INT2IP = 0;     /* set rc_dig_int1 low priority */
         INTCON3bits.INT2IF = 0;     /* clear int flag for rc_dig_int2 */
         INTCON3bits.INT2IE = TRUE;  /* enable rc_dig_int1 */
      } else {
         INTCON3bits.INT2IE = FALSE; /* disable rc_dig_int1 */
      }
      return;

   case 2:
      IntHandler2 = handler;
      if(IntHandler2)
      {
         INTCON2bits.INTEDG3 = 0;    /* set rc_dig_int2 falling edge triggered */
         INTCON2bits.INT3IP = 0;     /* set rc_dig_int2 low priority */
         INTCON3bits.INT3IF = 0;     /* clear int flag for rc_dig_int2 */
         INTCON3bits.INT3IE = TRUE;  /* enable rc_dig_int2 */
      } else {
         INTCON3bits.INT3IE = FALSE; /* disable rc_dig_int2 */
      }
      return;

   case 3:
      IntHandler3 = handler;
      TRISBbits.TRISB7 = INPUT;   // make sure the RB4/HBI0 pin is configured as an input [108]
      TRISBbits.TRISB6 = INPUT;   // make sure the RB4/HBI0 pin is configured as an input [108]
      TRISBbits.TRISB5 = INPUT;   // make sure the RB4/HBI0 pin is configured as an input [108]
      TRISBbits.TRISB4 = INPUT;   // make sure the RB4/HBI0 pin is configured as an input [108]
      //
      INTCON2bits.RBIP = 0;       // 0: interrupts 3-6 are low priority (leave at 0 for IFI controllers) [90]
      // 1: interrupts 3-6 are high priority
      //
      INTCONbits.RBIF = 0;        // 0: none of the interrupt 3-6 pins has changed state (set to 0 before enabling the interrupts) [89]
      // 1: at least one of the interrupt 3-6 pins has changed state
      //
      INTCONbits.RBIE = TRUE;     // disable interrupts 3-6 [89]
      // enable interrupts 3-6
      return;

   case 4:
      IntHandler4 = handler;
      TRISBbits.TRISB5 = INPUT;   // make sure the RB5/KBI1/PGM pin is configured as an input [108]
      //
      INTCON2bits.RBIP = 0;       // 0: interrupts 3-6 are low priority (leave at 0 for IFI controllers) [90]
      // 1: interrupts 3-6 are high priority
      //
      INTCONbits.RBIF = 0;        // 0: none of the interrupt 3-6 pins has changed state (set to 0 before enabling the interrupts) [89]
      // 1: at least one of the interrupt 3-6 pins has changed state
      //
      INTCONbits.RBIE = TRUE;     // disable interrupts 3-6 [89]
      // enable interrupts 3-6
      return;

   case 5:
      IntHandler5 = handler;
      TRISBbits.TRISB6 = INPUT;   // make sure the RB6/KBI2/PGC pin is configured as an input [108]
      //
      INTCON2bits.RBIP = 0;		// 0: interrupts 3-6 are low priority (leave at 0 for IFI controllers) [90]
      // 1: interrupts 3-6 are high priority
      //
      INTCONbits.RBIF = 0;		// 0: none of the interrupt 3-6 pins has changed state (set to 0 before enabling the interrupts) [89]
      // 1: at least one of the interrupt 3-6 pins has changed state
      //
      INTCONbits.RBIE = TRUE;     // disable interrupts 3-6 [89]
      // enable interrupts 3-6
      return;

   case 6:
      IntHandler6 = handler;
      TRISBbits.TRISB7 = INPUT;   // make sure the RB7/KBI3/PGD pin is configured as an input	[108]
      //
      INTCON2bits.RBIP = 0;		// 0: interrupts 3-6 are low priority (leave at 0 for IFI controllers) [90]
      // 1: interrupts 3-6 are high priority
      //
      INTCONbits.RBIF = 0;		// 0: none of the interrupt 3-6 pins has changed state (set to 0 before enabling the interrupts) [89]
      // 1: at least one of the interrupt 3-6 pins has changed state
      //
      INTCONbits.RBIE = TRUE;	    // disable interrupts 3-6 [89]
      // enable interrupts 3-6
      return;

#ifdef TIMER_INTS

   case TIMER_INT_1:
      TmrIntHandler1 = handler;
      TMR1H = TMR1L = 0;          /* reset timer registers */
      if(TmrIntHandler1)
      {
         IPR1bits.TMR1IP = 0;    /* set timer 1 low priority */
         PIR1bits.TMR1IF = 0;
         PIE1bits.TMR1IE = TRUE;
         T1CONbits.TMR1ON = 1;
      } else {
         PIE1bits.TMR1IE = FALSE;    /* disable timer 1 interrupt */
         T1CONbits.TMR1ON = 0;
      }
      PIR1bits.TMR1IF = 0;    /* clear int flag for timer 1 */

      return;

   case TIMER_INT_3:
      TmrIntHandler3 = handler;
      TMR3H = TMR3L = 0;          /* reset timer registers */
      if(TmrIntHandler3)
      {
         IPR2bits.TMR3IP = 0;    /* set timer 3 low priority */
         PIR2bits.TMR3IF = 0;    /* clear int flag for timer 3 */
         PIE2bits.TMR3IE = TRUE;
         T3CONbits.TMR3ON = 1;
      } else {
         PIE2bits.TMR3IE = FALSE;    /* disable timer 3 */
         T3CONbits.TMR3ON = 0;
      }

      return;

#endif

   default:
      printf("Unknown interrupt: %d\n", intNum);
      break;
   }
}

/*******************************************************************************
* FUNCTION NAME: InterruptVectorLow
* PURPOSE:       Low priority interrupt vector
* CALLED FROM:   nowhere by default
* ARGUMENTS:     none
* RETURNS:       void
* DO NOT MODIFY OR DELETE THIS FUNCTION 
*******************************************************************************/
#pragma code InterruptVectorLow = LOW_INT_VECTOR
void InterruptVectorLow (void)
{
   _asm
      goto InterruptHandlerLow  /*jump to interrupt routine*/
      _endasm
}


/*******************************************************************************
* FUNCTION NAME: InterruptHandlerLow
* PURPOSE:       Low priority interrupt handler
* If you want to use these external low priority interrupts or any of the
* peripheral interrupts then you must enable them in your initialization
* routine.  Innovation First, Inc. will not provide support for using these
* interrupts, so be careful.  There is great potential for glitchy code if good
* interrupt programming practices are not followed.  Especially read p. 28 of
* the "MPLAB(R) C18 C Compiler User's Guide" for information on context saving.
* CALLED FROM:   this file, InterruptVectorLow routine
* ARGUMENTS:     none
* RETURNS:       void
*******************************************************************************/
#pragma code
#pragma interruptlow InterruptHandlerLow save=PROD /* You may want to save additional symbols. */

unsigned char Old_Port_B = 0xFF;// state of port b the last time
void InterruptHandlerLow ()     
{
   if (INTCON3bits.INT2IF && INTCON3bits.INT2IE)       /* The INT2 pin is RB2/DIG I/O 1. */
   { 
      INTCON3bits.INT2IF = 0;
      if(IntHandler1)
      {
         (*IntHandler1)(0x01);
      }
   }
   else if (INTCON3bits.INT3IF && INTCON3bits.INT3IE)  /* The INT3 pin is RB3/DIG I/O 2. */
   {
      INTCON3bits.INT3IF = 0;
      if(IntHandler2)
      {
         (*IntHandler2)(0x02);
      }
   }
   else if (INTCONbits.RBIF && INTCONbits.RBIE)  /* DIG I/O 3-6 (RB4, RB5, RB6, or RB7) changed. */
   {
      unsigned char Port_B;
      unsigned char Port_B_Delta;
      Port_B = PORTB;                     /* You must read or write to PORTB   */
      INTCONbits.RBIF = 0;                // clear the interrupt flag [89]
      Port_B_Delta = Port_B ^ Old_Port_B; // determine which bits have changed
      Old_Port_B = Port_B;                // Save the prior bits to know what has changed
      if(Port_B_Delta & 0x10)
      {
         if(IntHandler3)
         {
            (*IntHandler3)(0x04);
         }
      }
      if(Port_B_Delta & 0x20)
      {
         if(IntHandler4)
         {
            (*IntHandler4)(0x08);
         }
      }
      if(Port_B_Delta & 0x40)
      {
         if(IntHandler5)
         {
            (*IntHandler5)(0x10);
         }
      }
      if(Port_B_Delta & 0x80)
      {
         if(IntHandler6)
         {
            (*IntHandler6)(0x20);
         }
      }
   }                                        /*     to clear the interrupt condition.  */

   else if ( (INTCONbits.TMR0IE) && (INTCONbits.TMR0IF) )
   {
      INTCONbits.TMR0IF = 0;
      HiResCounter++;
   }

#ifdef TIMER_INTS
   else if ( (PIE1bits.TMR1IE) && (PIR1bits.TMR1IF) )
   {
      PIR1bits.TMR1IF = 0;
      if(TmrIntHandler1)
      {
         (*TmrIntHandler1)(0x02);
      }
   }
   else if ( (PIE2bits.TMR3IE) && (PIR2bits.TMR3IF) )
   {
      PIR2bits.TMR3IF = 0;
      if(TmrIntHandler3)
      {
         (*TmrIntHandler3)(0x08);
      }
   }
   else if ( (PIE3bits.TMR4IE) && (PIR3bits.TMR4IF) )
   {
      PIR3bits.TMR4IF = 0;
      HandleTimer();
   }
#endif

   else if ( (PIE1bits.TX1IE) && (PIR1bits.TX1IF) )
   {
      Tx_1_ISR();
   }
   else if ( (PIE1bits.RC1IE) && (PIR1bits.RC1IF) )
   {
      Rx_1_ISR();
   }
   else if ( (PIE3bits.TX2IE) && (PIR3bits.TX2IF) )
   {
      Tx_2_ISR();
   }
   else if ( (PIE3bits.RC2IE) && (PIR3bits.RC2IF) )
   {
      Rx_2_ISR();
   }
}

/******************************************************************************/
/******************************************************************************/
/******************************************************************************/
